
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintWriter;
import java.util.Arrays;
import java.util.Random;

public class PSO {

    //  Enxame de partículas. swarm[npt][n][2] - particula -> eixo -> 0:posicao 1:velocidade
    public double[][][] swarm;
    // Solução da partícula.
    public double fitness[];
//  Melhor posição já encontrada pela partícula  .
    public double[][][] best_swarm;
    public double[] best_fitness;
//  Número de dimensões do problema.  
    public int n;
//  Tamanho do enxame.
    public int npt = 40;
//  Velocidade máxima da partícula.
    public double vMax;
//  Erro do critério de parada.
    public double eps = 1E-7;
//  Coeficiênte social.
    public float c1 = 2;
//  Coeficiênte cognitivo.
    public float c2 = 2;
//  Peso inercial.
    public float w;
//  Peso inercial inicial.
    public float wi = 0.9f;
//  Melhores posições já encontradas pelo enxame.
    public double[][] fstar;
    public double[] fstar_fitness;
//  Melhor partícula da iteração atual.
    public double[][] lBest;
    public double lBest_fitness;
//  Melhor partícula do enxame.
    public double[][] gBest;
    public double gBest_fitness;
//  Número de chamadas da função objetivo.
    public int ncall = 0;
//  Número de melhores posições para fstar[].
    public int nps = 4;
//  Número de iterações do algorimto.
    public int kiter = 1;
//  Número máximo de iterações do algoritmo.
    public int niter = 1000;
//  Gera números aleatórios  
    public Random random = new Random();
//  Função objetivo.
    public int function;

    public PSO(float wf, int dimension, int swarm_size, int iterations, int function, float ylow, float yup) {
        //Parâmetros do construtor
        this.wi = wf;
        this.n = dimension;
        this.npt = swarm_size;
        this.function = function;
        this.niter = iterations;
        this.ylow = ylow;
        this.yup = yup;

        this.vMax = (this.yup - this.ylow) / 2;
        this.swarm = new double[npt][n][2];
        this.best_swarm = new double[npt][n][2];
        this.fitness = new double[npt];
        this.best_fitness = new double[npt];
        this.lBest = new double[n][2];
        this.gBest = new double[n][2];
        this.fstar = new double[nps][n];
        this.fstar_fitness = new double[nps];
        this.lBest_fitness = Double.MAX_VALUE;
        this.gBest_fitness = Double.MAX_VALUE;
        for (int i = npt; i < 10; i++) {
            this.best_fitness[i] = Double.MAX_VALUE;
        }
        for (int i = 0; i < nps; i++) {
            fstar_fitness[i] = Double.MAX_EXPONENT;
        }
    }

    public void optimize() throws IOException {
        //Arquivo de saída 
        File saida = new File("E:\\Dropbox/UFRGS/Disciplinas/Análise e Projeto de Algoritmos -/Projeto -", "results_convergence" + "" + ".txt");
        FileWriter writer = new FileWriter(saida, true);
        PrintWriter printer = new PrintWriter(writer);
        printer.println();
        printer.println("function: " + function + " dimension: " + n);
        //  Inicializar enxame
        for (int i = 0; i < npt; i++) {
            for (int j = 0; j < n; j++) {
                swarm[i][j][0] = (2 * random.nextDouble() - 1) * vMax;
                swarm[i][j][1] = (this.yup - this.ylow) * random.nextDouble() + this.ylow;
            }
            fitness[i] = eval(swarm[i], function);
        }
        // iniciar o processo de busca
        while ((kiter < niter)) { // considerar o erro no while
            // calcula as soluções encontradas
            for (int i = 0; i < npt; i++) {
                for (int j = 0; j < n; j++) {
                    // Calcular Fitness 
                    fitness[i] = eval(swarm[i], function);
                    ncall++;

                    //atualiza o melhor fitness da partícula
                    if (fitness[i] < best_fitness[i]) {
                        best_fitness[i] = fitness[i];
                        best_swarm[i] = swarm[i];
                    }
                    // atualiza o melhor fitness global
                    if (gBest_fitness > fitness[i]) {
                        gBest_fitness = fitness[i];
                        gBest = swarm[i];
                        fstar_fitness[nps - 1] = gBest_fitness;
                        Arrays.sort(fstar_fitness);
                    }
                }
            }

            // Atualizar peso inercial de acordo com os autores
            w = (float) (wi * Math.pow(1.00002, -kiter));
//            System.out.println("w: " + w);

            // Atualiza posição e velocidade        
            for (int i = 0; i < npt; i++) {
                for (int j = 0; j < n; j++) {
                    // Atualiza velocidade
                    swarm[i][j][1] = w * swarm[i][j][1] + c1 * random.nextDouble() * (best_swarm[i][j][0] - swarm[i][j][0])
                            + c2 * random.nextDouble() * (gBest[j][0] - swarm[i][j][0]);
                    // Atualiza posicao
                    swarm[i][j][0] = swarm[i][j][0] + swarm[i][j][1];
                    // Controle de velocidade máxima
                    if ((swarm[i][j][0] < ylow) || (swarm[i][j][0] > yup)) {
                        swarm[i][j][0] = (yup - ylow) * random.nextDouble() + ylow;
                        swarm[i][j][1] = swarm[i][j][1] * -0.5;
                    }
                }
            }

            //Critério de parada do erro
            double soma = 0d;
            for (int i = 0; i < nps; i++) {
                soma += Math.pow((fstar_fitness[i] - gBest_fitness), 2);
            }
            soma = Math.sqrt(soma);
            if (soma < eps) {
                printer.println();
                printer.flush();
                writer.close();
                return;
            }
            // Calcular erro
            kiter++;
            printer.println(kiter + "\t" + gBest_fitness);
        }
        printer.println();
        printer.flush();
        writer.close();
    }
    /* 
     * 
     * Função Objetivo
     * 
     */
    public float ylow;
    public float yup;

    public double eval(double[][] particle, int wich) {
        switch (wich) {
            case 1: {
//                Sphere
                double f = 0d;
                for (int i = 0; i < n; i++) {
                    f += Math.pow(particle[i][0], 2);
                }
                return f;
            }
            case 2: {
//                Schwefel's Problem 1.2

                double prev_sum, curr_sum, outer_sum;

                curr_sum = particle[0][0];
                outer_sum = (curr_sum * curr_sum);

                for (int i = 1; i < n; i++) {
                    prev_sum = curr_sum;
                    curr_sum = prev_sum + particle[i][0];
                    outer_sum += (curr_sum * curr_sum);
                }
                return outer_sum;

            }
            case 3: {
//              Generalized Rosenbrock's function
                double f = 0d;
                for (int i = 0; i < n - 1; i++) {
                    f += ((100.0 * (Math.pow(particle[i + 1][0] - (Math.pow(particle[i][0], 2)), 2)))
                            + Math.pow((1.0 - particle[i][0]), 2));
                }
                return f;
            }
            case 4: {
//                Ackley's function
                double a = 20.0, b = 0.2, c = 2.0 * Math.PI;
                double sum1 = 0.0;
                double sum2 = 0.0;
                for (int i = 0; i < n; i++) {
                    double xi = particle[i][0];
                    sum1 += (xi * xi);
                    sum2 += Math.cos(c * xi);
                }
                return (-a * Math.exp(-b * Math.sqrt(sum1 / n))) - Math.exp(sum2 / n) + a + Math.E;
            }
            case 5: {
//              Generalized Griewank function
                double sum = 0.0;
                double prod = 1.0;
                for (int i = 1; i <= n; i++) {
                    double xi = particle[i - 1][0];
                    sum += (xi * xi);
                    prod *= Math.cos(xi / Math.sqrt(i));
                }
                return (sum / 4000.0) - prod + 1.0;
            }
            case 6: {
//              Generalized Penalized function
                double sum = 0.0d;

                for (int i = 0; i < n - 1; i++) {
                    sum += Math.pow((y_i(particle[i][0]) - 1), 2)
                            //                            * (1 + 10 * Math.pow(Math.sin(Math.PI * y_i(particle[i + 1][0])), 2) + (y_i(Math.pow(particle[n - 1][0] - 1, 2))));
                            * (1 + 10 * Math.pow(Math.sin(Math.PI * y_i(particle[i + 1][0])), 2) + Math.pow(y_i(particle[n - 1][0] - 1), 2));
                }
                sum = (Math.PI / n) * (10 * Math.pow(Math.sin(Math.PI * y_i(particle[0][0])), 2) + sum);
                for (int i = 0; i < n; i++) {
                    sum += u_1(particle[i][0], 10, 100, 4);
                }
                return sum;
            }
        }
        return 0;
    }

    private double y_i(double x) {
        double d = 1.0 + ((1.0 / 4.0) * (x + 1.0));
        return d;
    }

    private double u_1(double x, double a, double k, double m) {
        if (x > a) {
            return k * Math.pow((x - a), m);
        } else if (x < -a) {
            return k * Math.pow((-x - a), m);
        } else {
            return 0;
        }
    }
}